
#include "Data/Data.hpp"

#define _EPS 0.01
#define NB_K  1 
#define P_K 1
#define MARG 0
#define MAXITK 80
#define THRES 1
template<class Kernel, class Resample>
class Particle<Kernel, Resample, Density::GeomBridge>{

public:
	Particle(Kernel *K,Resample *R,Density::GeomBridge *D,int M, Distribution::Distribution *F){
		_p=(*D).Get_p();
		mat Xt(M,_p);//p le nombre de parametre a obtenir de Density
		W=new double[M];
		X=Xt;
		_K=K;
		_R=R;
		_F=F;
		_Z=0;
		_D=D;
		kk=-1;
		_thres=_C*M;
		_M=M;
		Normalize();
		y=X.t();
		_n=(*D).Get_n();
		O2=new Data<ofstream>("Tempering.txt");	
		_K->Set_n(M);
	//	cout << "///" << _n << "///";
	}
	Particle(){
		cout << "Hidee2!";
	}
	Particle(const Particle& X)
	{
		cout << "Hidee!";
	}
	~Particle(){
	//	delete[] W;
	}
	void Filter()
	{
		this->Init();
		int i=1;
		double p=0; 
		//for(int i=1;i<(_n);i++)
	//	i{
		double b=(*_D).Get_bn();
	//	b+=(double)(1/(double)(_P-1));
		_temp=StepLength(b);
		Phiv.push_back(_temp);
		b+=_temp;
		cout << "b: " << b << " p";
		_D->Set_Phi(b);
		y=X.t();
		_D->Weight(y,W,i);//le p designera comment on avance 
		double ess=Ess_W(W,1);
		cout << " ESS: " << ess;
		X=y.t();
		p=_D->Get_bn();
		while(p<1){
	//		cout << i;
			cout << " step: " << i << "\n";
			this->Step(i);
			p=_D->Get_bn();
		}
		(*O2).Close();
		
	}
	void Init()
	{
		//pior
		X=_F->scrambled(_M);	
		Write();
		//cout << X;	
	}
	void Write()
	{
		for(int i=0;i<_M;i++)
		{
			std::ostringstream oss;
			oss << X(i,MARG);
			string s=oss.str();
			(*O2).Write(s);
			(*O2).Write(" ");

		}
			(*O2).Write("\n");
	}
	void Step(int i)
	{
		//cout << "Ess:" << Ess << "\n";
				//cout << "\\";
//		if(Ess< _thres)
//		{
			mat v=growingvect(_M);
			double *w=new double[_M];
			for(int j=0;j<_M;j++){
				 w[j]=W[j];
			//	cout << w[j] << "\n";
			}
			(*_R)(&_M,w,v);
			delete[] w;
			//cout << v << "\n";
			
			Arangemat(X,v);
			Normalize();//poids fixé a 1
			Write();
			//mat C=cov(X);
			//cout << C;
			//cout << "sum: "  << sum(X,0) << "\n";
			//cout << "before:" << X.row(1);
	//		for(int k=0;k<3;k++)
	//		{	
			
			cout << "\nmean " << mean(X,0) << "\n";
			_K->Set_bool(1);
			double prop_moved=0;
			int kk=0;
			double acc=1;
			_K->Set_s(X);
			mat Z=X;
			mat X0=X;
			_K->Move(&(Z),i);
			acc=_K->Get_accept();

			while(acc<0.15 |acc>0.5)
			{
				if(acc<0.15)
				{
					_K->Set_Sigma(0.66);
				}else if(acc>0.5){
			//	cout << "Hello\n" << Acc[Acc.size()];
					_K->Set_Sigma(1.5);
				}
				cout << "....";
				try{
					_K->Set_s(X);
				
				}
				catch(std::exception& e)
				{
					cout << "chol did not converge, this step is not adaptative";
					
				}
				Z=X;
				_K->Move(&(Z),i);

				acc=_K->Get_accept();
				//cout << "acc " << acc;
				
			}
			X=Z;
			double dd0=0;
			double dd1=Distance(X0,X);
			cout << "Distance "  <<dd1 << "\n";
			_K->Set_bool(0);
			//while(sqrt(pow(dd1-dd0,2))>THRES) 
			while(prop_moved<P_K & kk<MAXITK)
			//for(int o=0;o<NB_K-1;o++)
			{	
				_K->Move(&(X),i);
				_K->Set_bool(0);
				dd0=dd1;
				dd1=Distance(X0,X);
				prop_moved=_K->nMove();
				cout << dd1;
				kk++;
			}
			prop_moved=_K->nMove();
			cout << "***********" << prop_moved << "*******\n";
			Acc.push_back(_K->Get_accept());
			cout << "\nmean " << mean(X,0) << "\n";
			kk++;	
		/*	if(Acc[kk]<0.15)
			{
				_K->Set_Sigma(0.75);
			}else if(Acc[kk]>0.45){
			//	cout << "Hello\n" << Acc[Acc.size()];
				_K->Set_Sigma(1.5);
			}	
			*/
			
			//		}
			//cout << "after:" << X.row(1);

//		}
				
		y=X.t();
		//cout << X << "////";	
		double b=_D->Get_bn();
	//	b+=(double)(1/(double)(_P-1));
		double etas=Ess_W(_D->Eta_star(_temp,W,y),0)/_M;
		cout << "etas :"<< etas;
		if(etas>_C+_EPS | etas<_C-_EPS)
		{
			_temp=StepLength(b);
		}else{
			_temp=min(_temp,1-b);
		}
		Phiv.push_back(_temp);
		b+=_temp;
		cout << "b: " << b << "p";
		_D->Set_Phi(b);

		//cout<< "before:" << W[0] ;
		//cout<< "after:" << W[0] ;
		//dans le cas de tempering peut etre determiner
		_D->Weight(y,W,i);//le p designera comment on avance 
	//	cout << "after:" << y.col(1);
		X=y.t();
		double ess=Ess_W(W,1);
		cout << " ESS: " << ess;
		//cout << " ESS: " << Ess_W(W,1);
	}

	double Ess_W(double *w, int tf)//renormalisation???
	{
		double *foo= new double[_M];
		double sum=0;

		for(int i=0;i<_M;i++)
		{
		//	cout << w[i] << " ";
			sum+=w[i];
		}
		if(tf){
			_Z+=log(sum/_M);
		}
		for(int j=0;j<_M;j++)
		{
			foo[j]=(w[j]/sum);
		}

		sum=0;
		for(int i=0;i<_M;i++)
		{
			sum+=foo[i]*foo[i];
		}
		delete[] foo;

		return 1/sum;		
	}
	double StepLength(double phi)
	{
		double u=10-phi;
		double l=0;
		double thres=0.000001;
		double e=2;
		double alpha=0.01;
		double eta=0;	
		while((e>thres)&(l<1-phi))
		{
			mat y=X.t();
			//cout << "eta"; 
			//for(int i=0;i<_M;i++)
			//{
			 //      	cout << [i] << "\n";
			//}
			eta=Ess_W(_D->Eta_star(alpha,W,y),0)/_M;
			if(eta>_U)
			{
				l=alpha;
				alpha=(double)(alpha+u)/2;
			}else{
				u=alpha;
				alpha=(double)(alpha+l)/2;

			}	
			e=abs<double>(u-l);
			cout << "\\\\\\ "<< eta <<" \\\\\\\\\n"; 
		}
		return min2<double>(alpha,1-phi);
	}
	double Distance(mat X0, mat Xi)
	{
		double mean=0;
		for(int i=0;i<_M;i++)
		{
			mean+=Norm<rowvec>(X0(i,span::all),Xi(i,span::all));			
		}
		return mean/_M;
	}

	
	mat Get_theta(void){return X;}
	void Normalize(void){
		for(int i=0;i<_M;i++){
			W[i]=1;
		}
	}
	mat   Get_W(void){
		double sum=0;
		mat C(_M,1);
		for(int i=0;i<_M;i++)
		{
			sum+=W[i];
		}	
		for(int i=0;i<_M;i++)
		{
			C(i,0)=(W[i]/sum)*_M;
		}
//		cout << C;
		return C;
	}
	double Get_Z(void){
		cout << "ev: " << _Z;       
		return _Z;}
	mat Get_Phiv(void)
	{
		int n=Phiv.size();
		mat res(n,1);
		for(int i=0;i<n;i++)
		{
			res(i,0)=Phiv[i];
		}
		return res; 
	}
	mat Get_Acc(void)
	{
		int n=Acc.size();
		mat res(n,1);
		for(int i=0;i<n;i++)
		{
			res(i,0)=Acc[i];
		}
		return res; 
	}

private:
	mat X;//value particle
	double *W;//weigths
	double Ess;
	int _M;
	double _thres;
	double _Z;
	Kernel *_K;
	Resample *_R;
	Density::GeomBridge *_D;	
	Distribution::Distribution *_F;
	mat y;
	double _temp;
	int _n;
	int _p;
	int kk;
	Data<ofstream> *O2;
	vector<double> Phiv;
	vector<double> Acc;
};
class Normal : public Distribution
{
	public:
		Normal(int n,mat m, mat s){
			_n=n;

		}
}
